#!/usr/bin/python 
# -*- coding: utf-8 -*-                 #通过声明可以在程序中书写中文
import RPi.GPIO as GPIO                 #引入RPi.GPIO库函数命名为GPIO
import time                             #引入计时time函数
import pulse

# BOARD编号方式，基于插座引脚编号
GPIO.setmode(GPIO.BOARD)                #将GPIO编程方式设置为BOARD模式

#接口定义
INT1 = 11                               #将L298 INT1口连接到树莓派Pin11
INT2 = 12                               #将L298 INT2口连接到树莓派Pin12
INT3 = 13                               #将L298 INT3口连接到树莓派Pin13
INT4 = 15                               #将L298 INT4口连接到树莓派Pin15
ENA = 16
#输出模式
GPIO.setup(INT1,GPIO.OUT)
GPIO.setup(INT2,GPIO.OUT)
GPIO.setup(INT3,GPIO.OUT)
GPIO.setup(INT4,GPIO.OUT)
#GPIO.setup(ENA,GPIO.OUT)

#pwma = GPIO.PWM(16,80)
#pwma.start(90)
tslp=0.5

DISTINCT = 20

def init():
	GPIO.setmode(GPIO.BOARD)                #将GPIO编程方式设置为BOARD模式

	#接口定义
	INT1 = 11                               #将L298 INT1口连接到树莓派Pin11
	INT2 = 12                               #将L298 INT2口连接到树莓派Pin12
	INT3 = 13                               #将L298 INT3口连接到树莓派Pin13
	INT4 = 15                               #将L298 INT4口连接到树莓派Pin15
	ENA = 16
	#输出模式
	GPIO.setup(INT1,GPIO.OUT)
	GPIO.setup(INT2,GPIO.OUT)
	GPIO.setup(INT3,GPIO.OUT)
	GPIO.setup(INT4,GPIO.OUT)	
	
def stop():
	GPIO.cleanup()

#前进
def up():
	init()
	print("Measured Distance = {:.2f} cm".format(pulse.distance()))
	if pulse.distance() <  DISTINCT :
		print('stop distinct') 
		GPIO.cleanup()
		return 
	GPIO.output(INT1,GPIO.HIGH)
	GPIO.output(INT2,GPIO.LOW)
	GPIO.output(INT3,GPIO.HIGH)
	GPIO.output(INT4,GPIO.LOW)
	time.sleep(0.2)
	print('motor up')
	GPIO.cleanup()

#后退

def down():
	init()
	GPIO.output(INT1,GPIO.LOW)
	GPIO.output(INT2,GPIO.HIGH)
	GPIO.output(INT3,GPIO.LOW)
	GPIO.output(INT4,GPIO.HIGH)
	time.sleep(0.2)
	GPIO.cleanup()

#向左
def left():
	init()
	GPIO.output(INT1,GPIO.LOW)
	GPIO.output(INT2,GPIO.HIGH)
	GPIO.output(INT3,GPIO.HIGH)
	GPIO.output(INT4,GPIO.LOW)
	time.sleep(0.2)
	GPIO.cleanup()

#向右
def right():
	init()
	GPIO.output(INT1,GPIO.HIGH)
	GPIO.output(INT2,GPIO.LOW)
	GPIO.output(INT3,GPIO.LOW)
	GPIO.output(INT4,GPIO.HIGH)
	time.sleep(0.2)
	GPIO.cleanup()
